#ifndef ROSNODE_H
#define ROSNODE_H

#include <QObject>
#include "ros/ros.h"
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/PointCloud2.h>
#include "tools/Singleton.h"
#include <map>
#include "util/topicfiled.h"


class RosNode : public QObject
{
public:
    enum  PoseSelect{
        PoseSelect_Succ =1,
        PoseSelect_NotInMap =2,
        PoseSelect_InObstacle = 3
    };

    Q_OBJECT
public:
    explicit RosNode(QObject *parent = nullptr);

    ~RosNode();

    void subMap();
    void pubMap(nav_msgs::OccupancyGrid& map);
    nav_msgs::OccupancyGrid& getMap();
    void callback(const nav_msgs::OccupancyGrid::ConstPtr& msg);
    void callback(const sensor_msgs::PointCloud2::ConstPtr& msg, QString topic);

    //订阅topoc
    template <typename T>
    void sub(QString topic);

    //获取topic回调内容
    template <typename T>
    T& get(QString topic);

    //回调
    template <typename  T>
    void callBack(const typename T::ConstPtr& msg, QString topic);


    void subPose();
    void subPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg, int i);

    void pubPose();
    void getPos(std::vector<geometry_msgs::PoseWithCovarianceStamped>& vecPos);

    void subPath();
    void pubPath(std::vector<nav_msgs::Path>& path);

    void subPoint2(QString topic);
    sensor_msgs::PointCloud2* getPoint2Data(QString topic);


    //历史轨迹
    void pubMarker(std::vector<nav_msgs::Path>& path);
    void pubMarker(nav_msgs::Path& path,int markId,int32_t  type, QColor& color);



    void pubPoint2(sensor_msgs::PointCloud2 &p);



    bool isObstacle(unsigned int x, unsigned int y);
private:
    bool worldToMapGrid(double world_x, double world_y,int& grid_x, int& grid_y);

signals:
    void sigPoseSelectState(PoseSelect,int);
    void sigTopicRecive(QString topic);

private:
    ros::NodeHandle n;
    nav_msgs::OccupancyGrid m_map;
    ros::Subscriber m_subMap;
    ros::Publisher  m_pubMap;
    ros::Publisher m_pubMarker;
    ros::Subscriber m_subMarker;
    ros::Publisher m_pubPoint2;

    std::vector<ros::Subscriber> m_vecPose;
    std::vector<ros::Publisher> m_vecPubPose;
    std::vector<ros::Publisher> m_vecPubPath;


    std::map<QString, ros::Subscriber> m_mapSub;
    std::map<QString, void*> m_mapData;




    std::map<int,geometry_msgs::PoseWithCovarianceStamped> m_mapPose;
};


#define GRosNode Singleton<RosNode>::getInstance()

#endif // ROSNODE_H
